/*******************************************************************************
 * Team: R2-DTurrets
 * ECEN 4013 Senior Design 1
 *
 * Turret Code
 *
 * Authors: Matt Polito - Project Manager
 *          Mike Krueger - Lead Engineer
 *          Kevin Bright - Motion Master
 *          Shane Ptomey - Power Sage
 *          Kali McCuistion - Visual and Audio Doctor
 *          Christien Sager - Communication Analyst
 *          Wesley Taylor - Information Expert
 *
 * Special Thanks to Matthew Spinks for use of Servo Libriaries
*******************************************************************************/

// ***** Include External Files *****************************************************
#include <xc.h>
#include <pic18F4580.h>
#include "global_defs.h"
#include "delays.h"

// ***** PIC18F4580 Configuration Bit Settings **************************************
// CONFIG1H
#pragma config OSC = IRCIO67     // Oscillator Selection bits (Internal oscillator block, port function on RA6 and RA7)
#pragma config FCMEN = OFF      // Fail-Safe Clock Monitor Enable bit (Fail-Safe Clock Monitor disabled)
#pragma config IESO = OFF       // Internal/External Oscillator Switchover bit (Oscillator Switchover mode disabled)
// CONFIG2L
#pragma config PWRT = OFF       // Power-up Timer Enable bit (PWRT disabled)
#pragma config BOREN = BOHW     // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware only (SBOREN is disabled))
#pragma config BORV = 3         // Brown-out Reset Voltage bits (VBOR set to 2.1V)
// CONFIG2H
#pragma config WDT = OFF        // Watchdog Timer Enable bit (WDT disabled (control is placed on the SWDTEN bit))
#pragma config WDTPS = 32768    // Watchdog Timer Postscale Select bits (1:32768)
// CONFIG3H
#pragma config PBADEN = ON      // PORTB A/D Enable bit (PORTB<4:0> pins are configured as analog input channels on Reset)
#pragma config LPT1OSC = OFF    // Low-Power Timer 1 Oscillator Enable bit (Timer1 configured for higher power operation)
#pragma config MCLRE = ON       // MCLR Pin Enable bit (MCLR pin enabled; RE3 input pin disabled)
// CONFIG4L
#pragma config STVREN = ON      // Stack Full/Underflow Reset Enable bit (Stack full/underflow will cause Reset)
#pragma config LVP = OFF        // Single-Supply ICSP Enable bit (Single-Supply ICSP disabled)
#pragma config BBSIZ = 1024     // Boot Block Size Select bit (1K words (2K bytes) boot block)
#pragma config XINST = OFF      // Extended Instruction Set Enable bit (Instruction set extension and Indexed Addressing mode disabled (Legacy mode))
// CONFIG5L
#pragma config CP0 = OFF        // Code Protection bit (Block 0 (000800-001FFFh) not code-protected)
#pragma config CP1 = OFF        // Code Protection bit (Block 1 (002000-003FFFh) not code-protected)
#pragma config CP2 = OFF        // Code Protection bit (Block 2 (004000-005FFFh) not code-protected)
#pragma config CP3 = OFF        // Code Protection bit (Block 3 (006000-007FFFh) not code-protected)
// CONFIG5H
#pragma config CPB = OFF        // Boot Block Code Protection bit (Boot block (000000-0007FFh) not code-protected)
#pragma config CPD = OFF        // Data EEPROM Code Protection bit (Data EEPROM not code-protected)
// CONFIG6L
#pragma config WRT0 = OFF       // Write Protection bit (Block 0 (000800-001FFFh) not write-protected)
#pragma config WRT1 = OFF       // Write Protection bit (Block 1 (002000-003FFFh) not write-protected)
#pragma config WRT2 = OFF       // Write Protection bit (Block 2 (004000-005FFFh) not write-protected)
#pragma config WRT3 = OFF       // Write Protection bit (Block 3 (006000-007FFFh) not write-protected)
// CONFIG6H
#pragma config WRTC = OFF       // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) not write-protected)
#pragma config WRTB = OFF       // Boot Block Write Protection bit (Boot block (000000-0007FFh) not write-protected)
#pragma config WRTD = OFF       // Data EEPROM Write Protection bit (Data EEPROM not write-protected)
// CONFIG7L
#pragma config EBTR0 = OFF      // Table Read Protection bit (Block 0 (000800-001FFFh) not protected from table reads executed in other blocks)
#pragma config EBTR1 = OFF      // Table Read Protection bit (Block 1 (002000-003FFFh) not protected from table reads executed in other blocks)
#pragma config EBTR2 = OFF      // Table Read Protection bit (Block 2 (004000-005FFFh) not protected from table reads executed in other blocks)
#pragma config EBTR3 = OFF      // Table Read Protection bit (Block 3 (006000-007FFFh) not protected from table reads executed in other blocks)
// CONFIG7H
#pragma config EBTRB = OFF      // Boot Block Table Read Protection bit (Boot block (000000-0007FFh) not protected from table reads executed in other blocks)

// ***** Term Definition ************************************************************
#define snipeLED        PORTAbits.RA2   //Pin to signal a snipe call
#define blastLED        PORTAbits.RA3   //Pin to signal a blast call
#define fireBlast       PORTAbits.RA6   //S1 sound pin
#define fireSnipe       PORTAbits.RA7   //S0sound pin
#define servoX          PORTBbits.RB0   //Signal pin for controlling X servo motor
#define servoY          PORTBbits.RB1   //Signal pin for controlling Y servo motor
#define canbusTx        PORTBbits.RB2   //Transmit pin for CANbus
#define canbusRx        PORTBbits.RB3   //Receive pin for CANbus
#define unknownSound    PORTBbits.RB4   //E1 sound pin
#define startUp         PORTBbits.RB5   //E0 sound pin
#define BLANK           PORTCbits.RC0   //5th header on visuals header strip
#define SCLK            PORTCbits.RC1   //3rd header on visuals header strip
#define SIN             PORTCbits.RC2   //4th header on visuals header strip
#define XLAT            PORTCbits.RC3   //6th header on visuals header strip
#define IRR3            PORTCbits.RC4   //IRR3 is the fifth header on the IR receive header strip
#define IRR4            PORTCbits.RC5   //IRR4 is the sixth header on the IR receive header strip
#define blueRx          PORTCbits.RC6   //blueRx refers to the Tx pin on the UART of the microcontroller
#define blueTx          PORTCbits.RC7   //blueTx refers to the Rx pin on the UART of the microcontroller
#define attacked        PORTDbits.RD0   //S2 sound pin
#define ammoDepleted    PORTDbits.RD1   //S3 sound pin
#define IRR1            PORTDbits.RD2   //IRR1 is the third header on the IR receive header strip
#define IRR2            PORTDbits.RD3   //IRR2 is the fourth header on the IR receive header strip
#define reload          PORTDbits.RD6   //E3 sound pin
#define cooldown        PORTDbits.RD7   //E2 sound pin

#define usToTicks(us)       (us * 2)
#define tickstoUs(ticks)    (ticks / 2)
#define isServoActive(chan) (activeServos & (1 << chan))
#define delay_cycles    250000
#define REFRESH_US      20000
#define MAX_STEP        4095
#define SERVO_RESET     0
#define IR_R0 ((IRR1 == 0) || (IRR2 == 0) || (IRR3 == 0) || (IRR4 == 0))
#define IR_R1 ((IRR1 == 1) && (IRR2 == 1) && (IRR3 == 1) && (IRR4 == 1))

// ***** Variable Declarations ******************************************************
// *** IR Receive Variables ***
volatile unsigned long start_on = 0;
volatile unsigned long start_off = 0;
volatile unsigned long data_on = 0;
volatile unsigned long data_off = 0;
volatile unsigned long r_data_on = 0;
volatile unsigned long r_data_off = 0;
volatile unsigned long stop_on = 0;
volatile unsigned char start_pass = 0;
volatile unsigned char heal_pass = 0;
volatile unsigned char heal2_pass = 0;
volatile unsigned char hit_pass = 0;
volatile unsigned char hit2_pass = 0;
// *** Bluetooth Variables ***
unsigned char ID;
unsigned char dataRx;
unsigned char dataTx;
// *** Servo Motor Variables
unsigned char xAxis;
unsigned char xAxisH;
unsigned int xAxisM;
unsigned char yAxis;
unsigned char yAxisH;
unsigned int yAxisM;
unsigned char error;
volatile unsigned char channel = 0;
volatile unsigned char activeServos = 0;
volatile unsigned char servoFlags = 0;
volatile unsigned int servoValues[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
// *** Visuals Variables ***
unsigned int pwmbuffer[24];         //Brightness buffer
unsigned char sevenSegLookUp[10];   //digits 0-9
int b, c, i, j, k;         // Loop variables
// *** IR Transmit Variable ***
unsigned char IrTxControl;
// *** CANBUS Variable ***
unsigned char temp_EIDH;
unsigned char temp_EIDL;
unsigned char temp_SIDH;
unsigned char temp_SIDL;
unsigned char temp_DLC;
unsigned char temp_D0;
unsigned char temp_D1;

// ***** Function Declarations ******************************************************
// *** Servo Fuctions ***
void ServoInit(void);
void ServoInterrupt(void);
unsigned char ServoEnable(unsigned char);
unsigned char ServoDisable(unsigned char);
unsigned char ServoSetPulseUs(unsigned char, unsigned int);
unsigned int xMap(unsigned char xAxis);
unsigned int yMap(unsigned char yAxis);
// *** Bluetooth Functions ***
unsigned char checkID(unsigned char data);
unsigned char xHigh(unsigned char RX);
unsigned char xLow(unsigned char RX);
unsigned char yHigh(unsigned char RX);
unsigned char yLow(unsigned char RX);
// *** Audio Functions ***
void playSound(int i);
// *** IR Receive Functions ***
void IR_Receive(void);
// *** Visual Functions ***
void Cooldown(char color, int step, int loop);
void fadeLED(char color, int step);
void flashLED(char color, unsigned int time, int brightness);
void colorLED(int OUTn, int r, int g, int b);
void selectPWM(int chan, int pwm);
void writeLED(void);
void clearLED(void);
// *** IR Transmit ***
void blast(void);
void blastOn(unsigned char j);
void blastOff(unsigned char j);
void snipe(void);
void snipeOn(unsigned char j);
void snipeOff(unsigned char j);
// *** CANbus Functions ***
void CanInit(void);
void MCAN_Receive(void);
void send_Dead(void);

// ***** Main Function **************************************************************
void main(){
    //Initializing the Pic18 and sets the oscillator to 8 MHz
    OSCCONbits.IRCF2 = 1;
    OSCCONbits.IRCF1 = 1;
    OSCCONbits.IRCF0 = 1;
    OSCCONbits.SCS1 = 1;    //Timer1 oscillator disabled
    //Analog to Digital Settings
    ADCON0=0x00;        //sets ADC functions off
    ADCON1=0x0f;        //sets all pins to digital interfacing
    //Clearing PORTD for Digital Use
    ECCP1CONbits.ECCP1M = 0;    //Disables PWM mode on PORTD
    TRISEbits.PSPMODE = 0;      //Disable Parallel Slave Port for PORTD
    CMCON = 0x07;               //Disable Comparators on PORTD
    SSPCON1bits.SSPEN = 0;      //Disable SCK/SDO/SDI for PORTC
    CANCON = 0x00;              //Place CANbus on RB2 and RB3
    TXB0CONbits.TXREQ=0;        //Configures RB2 for CAN transmission
    //Set Baud Rate to 9600
    TXSTAbits.SYNC = 0;     //Set Eusart to Asynchronous Mode
    TXSTAbits.BRGH = 1;     //Low speed baud rate select
    BAUDCONbits.BRG16 = 0;  //8-bit baud rate generator selected
    SPBRG = 8;              //Value for 57600 baud rate
    // Set inputs and outputs
    TRISAbits.RA2 = 0;  //RA2 is output to enable blast firing
    TRISAbits.RA3 = 0;  //RA3 is output to enable snipe firing
    TRISAbits.RA6 = 0;  //RA6 is output for sound
    TRISAbits.RA7 = 0;  //RA7 is output for sound

    TRISBbits.RB0 = 0;  //RB0 is output for Xservo
    TRISBbits.RB1 = 0;  //RB1 is output for Yservo
    TRISBbits.RB2 = 0;  //RB2 is output for transmit for CANbus
    TRISBbits.RB3 = 1;  //RB3 is input for receive for CANbus
    TRISBbits.RB4 = 0;  //RB4 is output for sound
    TRISBbits.RB5 = 0;  //RB5 is output for sound

    TRISCbits.RC0 = 0;  //RC0 is output for Visuals
    TRISCbits.RC1 = 0;  //RC1 is output for Visuals
    TRISCbits.RC2 = 0;  //RC2 is output for Visuals
    TRISCbits.RC3 = 0;  //RC3 is output for Visuals
    TRISCbits.RC4 = 1;  //RC4 is input for IR receive
    TRISCbits.RC5 = 1;  //RC5 is input for IR receive
    TRISCbits.RC6 = 0;  //RC6 is output for Bluetooth Transmit
    TRISCbits.RC7 = 1;  //RC7 is input for Bluetooth Receive

    TRISDbits.RD0 = 0;  //RD0 is output for Sound
    TRISDbits.RD1 = 0;  //RD1 is output for Sound
    TRISDbits.RD2 = 1;  //RD2 is input for IR Receive
    TRISDbits.RD3 = 1;  //RD3 is input for IR Receive
    TRISDbits.RD4 = 1;  //RD4 is input for IR Receive
    TRISDbits.RD5 = 1;  //RD5 is input for IR Receive
    TRISDbits.RD6 = 0;  //RD6 is output for sound
    TRISDbits.RD7 = 0;  //RD7 is output for sound

    // *** Bluetooth Initialization ***
    RCSTAbits.SPEN = 1;  // Enables the serial port
    RCSTAbits.CREN = 1;  // Recieve enabled
    TXSTAbits.TXEN = 1;  // Transmit enabled

    // Initialize Sound Variables
    attacked = 1;
    ammoDepleted = 1;
    cooldown = 1;
    reload = 1;
    fireSnipe = 1;
    fireBlast = 1;
    startUp = 1;
    unknownSound = 1;
    BLANK = 1;

    CanInit();
    ServoInit();
    error = ServoEnable(0);
    error = ServoSetPulseUs(0,1500);
    error = ServoEnable(1);
    error = ServoSetPulseUs(1,700);

    while(1){
        
        dataRx = RCREG;

        while(IR_R0){
            IR_Receive();
        }
        ID = checkID(dataRx);
        while(IR_R0){
           IR_Receive();
        }

        if(ID == 0x10){
            while(IR_R0){
                IR_Receive();
            }
            xAxisH = xHigh(dataRx);
        }
        if(ID == 0x20){
            while(IR_R0){
                IR_Receive();
            }
            xAxis = xAxisH + xLow(dataRx);
        }
        if(ID == 0x30){
            while(IR_R0){
                IR_Receive();
            }
            yAxisH = yHigh(dataRx);
        }
        if(ID == 0x40){
            while(IR_R0){
               IR_Receive();
            }
            yAxis = yAxisH + yLow(dataRx);
        }

        // Check data received from bluetooth
        if(dataRx == 0x88){
            while(IR_R0){
                IR_Receive();
            }
            playSound(3);
            flashLED('y', 4095, 60000);
            while(IR_R0){
                IR_Receive();
            }
        }else if(dataRx == 0x55){
            while(IR_R0){
                IR_Receive();
            }
            flashLED('y', 4095, 60000);
            while(IR_R0){
                IR_Receive();
            }
        }
        else if(dataRx == 0x66){
            while(IR_R0){
                IR_Receive();
            }
            playSound(4);
            flashLED('b', 4095, 60000);
            while(IR_R0){
                IR_Receive();
            }
        }else if(dataRx == 0x77){
            while(IR_R0){
                IR_Receive();
            }
            flashLED('b', 4095, 60000);
            while(IR_R0){
                IR_Receive();
            }
        }else if(dataRx == 0xFF){
            while(IR_R0){
                IR_Receive();
            }
            snipe();
            snipe();
            snipe();
            snipe();
            snipe();
            snipe();
            snipe();
            snipe();
            snipe();
            playSound(5);
            flashLED('c', 4095, 60000);
            while(IR_R0){
                IR_Receive();
            }
        }else if(dataRx == 0xee){
           while(IR_R0){
               IR_Receive();
            }
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            blast();
            playSound(6);
            flashLED('m', 4095, 60000);
            while(IR_R0){
                IR_Receive();
            }
        }
        if(dataRx == 0xbb)
        {
            send_Dead();
        }

        xAxisM = xMap(xAxis);

        while(IR_R0){
            IR_Receive();
        }
         yAxisM = yMap(yAxis);

        while(IR_R0){
            IR_Receive();
        }

        error = ServoSetPulseUs(0, xAxisM);
        while(IR_R0){
            IR_Receive();
        }
        error = ServoSetPulseUs(1,yAxisM);
        while(IR_R0){
            IR_Receive();
        }
    } // End while loop
} // End main function


unsigned char checkID(unsigned char data){
    unsigned char checkHighBits = 0xf0;
    ID = data & checkHighBits;
    return ID;
}

unsigned char xHigh(unsigned char RX){
    unsigned char xH;
    xH = RX << 4;
    return xH;
}

unsigned char xLow(unsigned char RX){
    unsigned char xL;
    xL = RX & 0x0f;
    return xL;
}

unsigned char yHigh(unsigned char RX){
    unsigned char xH;
    xH = RX << 4;
    return xH;
}

unsigned char yLow(unsigned char RX){
    unsigned char xL;
    xL = RX & 0x0f;
    return xL;
}

unsigned int xMap(unsigned char xAxis){
    if(xAxis > 150){//135
        return 1300;
    }else if(xAxis < 110){//119
        return 1700;
    }else{
        return 1500;
    }
}

unsigned int yMap(unsigned char yAxis){

    if(yAxis <= 104){
        return 1000;
    }else if(yAxis > 104 && yAxis <= 110){
        return 1100;
    }else if(yAxis > 110 && yAxis <= 116){
        return 1200;
    }else if(yAxis > 116 && yAxis <=122){
        return 1300;
    }else if(yAxis > 122 && yAxis <= 134){
        return 1500;
    }else if(yAxis > 134 && yAxis <= 140){
        return 1700;
    }else if(yAxis > 140 && yAxis <= 146){
        return 1800;
    }else if(yAxis > 146 && yAxis <= 152){
        return 1900;
    }else if(yAxis > 152 && yAxis <= 160){
        return 2000;
    }
}

// ***** Interrupt Service Routine **************************************************

void interrupt ISR(void){
// ***** Timer 1 Compare Interrupt **************************************************
    if(CCP1IE && CCP1IF){
        CCP1IF = 0;         //clear interrupt flag
        ServoInterrupt();
    }
} // end ISR

void playSound(int i){
    switch (i){
        case 1:
            attacked = 0;   // Set pin low to play track
            __delay_ms(50); // Delay to ensure sound plays
            attacked = 1;   // Reset pin to high to stop track
            break;
        case 2:
            ammoDepleted = 0;   // Set pin low to play track
            __delay_ms(50);     // Delay to ensure sound plays
            ammoDepleted = 1;   // Reset pin to high to stop track
            break;
        case 3:
            cooldown = 0;   // Set pin low to play track
            __delay_ms(50); // Delay to ensure sound plays
            cooldown = 1;   // Reset pin to high to stop track
            break;
        case 4:
            reload = 0;     // Set pin low to play track
            __delay_ms(50); // Delay to ensure sound plays
            reload = 1;     // Reset pin to high to stop track
            break;
        case 5:
            fireSnipe = 0;     // Set pin low to play track
            __delay_ms(50); // Delay to ensure sound plays
            fireSnipe = 1;     // Reset pin to high to stop track
            break;
        case 6:
            fireBlast = 0;     // Set pin low to play track
            __delay_ms(50); // Delay to ensure sound plays
            fireBlast = 1;     // Reset pin to high to stop track
            break;
        default:
            break;
    }
    __delay_ms(10);
} //end playSound

void IR_Receive(void){
    // Receive START packet
    while(IR_R0){
        start_on++;
        if(start_on >= 1000){
            break;
        }
    }
    __delay_us(50);
    while(IR_R1){
        start_off++;
        if(start_off >= 1000){
            break;
        }
    }
    __delay_us(50);
    // Receive DATA packet
    while(IR_R0){
        data_on++;
        if(data_on >= 1000){
            break;
        }
    }
    __delay_us(50);
    while(IR_R1){
        data_off++;
        if(data_off >= 1000){
            break;
        }
    }

    __delay_us(50);
    // Receive REDUNDANT DATA packet
    while(IR_R0){
        r_data_on++;
        if(r_data_on >= 1000){
            break;
        }
    }
    __delay_us(50);
    while(IR_R1){
        r_data_off++;
        if(r_data_off >= 1000){
            break;
        }
    }

    // Receive STOP packet
    __delay_us(50);
    while(IR_R0){
        stop_on++;
        if(stop_on >= 1000){
            break;
        }
    }

    // Check START packet
    if((start_on >= 10) && (start_on <= 20)){
        start_pass = 1;
    }

    // Check DATA packet
    if((start_pass == 1) && (data_on >=25) && (data_on <= 33)){
        hit_pass = 1;
    }else if((start_pass == 1) && (data_on >= 35) && (data_on <= 45)){
        heal_pass = 1;
    }

    // Check REDUNDANT DATA packet
    if((hit_pass == 1) && (r_data_on >= 25) && (r_data_on <= 33)){
        hit2_pass = 1;
    }else if((heal_pass == 1) && (data_on >= 35) && (data_on <= 45)){
        heal2_pass = 1;
    }

    // Check STOP packet
    if((hit2_pass == 1) && (stop_on >= 100) && (stop_on <= 160)){
        // Valid hit
        TXREG = 0xab;
        TXREG = 0x00;
        playSound(1);
        flashLED('r', 4095, 60000);
        flashLED('r', 4095, 60000);
        flashLED('r', 4095, 60000);
        __delay_ms(10);
    }else if ((heal2_pass == 1) && (stop_on >= 100) && (stop_on <= 160)){
        // Valid heal
        TXREG = 0x99;
        TXREG = 0x00;
        playSound(2);
        flashLED('g', 4095, 60000);
        flashLED('g', 4095, 60000);
        flashLED('g', 4095, 60000);
        __delay_ms(10);
    }

    // Reset IR Receive variables
    start_on = 0;
    start_off = 0;
    data_on = 0;
    data_off = 0;
    r_data_on = 0;
    r_data_off = 0;
    stop_on = 0;
    start_pass = 0;
    heal_pass = 0;
    heal2_pass = 0;
    hit_pass = 0;
    hit2_pass = 0;

    RCSTAbits.CREN = 0;
    RCSTAbits.CREN = 1;

} // End IR Receive

/*******************************************************************************
 * Title: PIC16F883 Servo Library
 *
 * Author: Matthew Spinks
 *
 * File: adc.c
 *
 * Created on March 19, 2014, 4:12 PM
 *
 * Description:
 *  My servo library is some-what similar to the Arduino library although I've
 *  reduced the code greatly. I'm using the Timer 1 compare interrupt for the
 *  servo pulses. It will generate pulses for up to 8 servos on PORTB. Since
 *  everything is on PORTB, I use logical AND's, OR's and macros to simplify
 *  things. This means there are less nested if's and function calls than the
 *  Arduino library. My other option would have been to make a pin library like
 *  the Arduino, but my goal here is have fast operation, rather than
 *  compatibility with every Microchip processor in the known universe.
 *
*******************************************************************************/

// ***** Macros ****************************************************************

// Each timer tick is 0.5us (8 MHz / 4)


// The servo library will need to always update the timer regardless of whether
// the servo is on or not. Therefore, servo values should never be zero. Also,
// servo values are handled in microseconds for ease of use. The inline macros
// do the conversions.


// ***** Functions *************************************************************


// ----- Initialize ------------------------------------------------------------

void ServoInit(void)
{
    // Set Timer 1 source to Fosc/4. We are using the 8 MHz internal oscillator
    // T = 8 MHz / 4 / 65536) = 32.77 ms
    // F = 1 / 32.77 ms = 30.51 Hz

    // It's a little slower than typical (50 Hz), so we may want to add some
    // extra conditions in to help speed it up a little. With 8 servos, the
    // maximum time needed should be about 16 ms.

    TMR1CS = 0;                     // set clock source to Fosc / 4
    CCP1CONbits.CCP1M = 0b1010;     // generate interrupt on match
    CCP1IE = 1;                     // enable CCP interrupt
    PEIE = 1;                       // enable peripheral interrupts
    TMR1ON = 1;                     // enable the timer

    // Initialize SERVO_RESET high to ensure that the interrupt begins with
    // channel zero.
    servoFlags |= (1 << SERVO_RESET);

    // Don't forget to enable global interrupts!
    GIE = 1;
}

// ----- Enable Servo ----------------------------------------------------------

// Enables the servo on Port B. You need to have the pin set as an output
// first! I did this on purpose. Returns an error if it isn't an output.
unsigned char ServoEnable(unsigned char channel)
{
    // Check if the port is enabled (0 = output)
    if(channel > 7 || (TRISB & (1 << channel)))
        return 1; // return an error

    activeServos |= (1 << channel); // else set boolean
    return 0;
}

// ----- Disable Servo ---------------------------------------------------------

unsigned char ServoDisable(unsigned char channel)
{
    if(channel > 7)
        return 1; // return an error

    activeServos &= ~(1 << channel);
    return 0;
}

// ----- Set Servo Pulse -------------------------------------------------------

// Sets the servo pulse in microseconds
unsigned char ServoSetPulseUs(unsigned char channel, unsigned int pulse)
{
    if(pulse > 2000 || pulse < 1000)
       return 1;

    servoValues[channel] = pulse;
    return 0;
}

// ----- Handle Interrupt ------------------------------------------------------

// Normally, I would not advicse calling a function from an interrupt, but I
// want the code to be more portable.
void ServoInterrupt(void)
{
    // SERVO_RESET needs to be set high on power up to avoid issues with
    // subtracting one and doing bit masks.
    if(servoFlags & (1 << SERVO_RESET))
    {
        servoFlags &= ~(1 << SERVO_RESET);  // clear the flag
        TMR1 = 0;                           // reset the timer
    }
    else if(isServoActive((channel - 1)))
    {
        // If we are not on the first channel, we will set previous servo
        // low if it is enabled
        PORTB &= ~(1 << (channel - 1));
    }

    // Check current channel
    if(channel < 8)
    {
        // Set current servo high if enabled
        if(isServoActive(channel))
           PORTB |= (1 << channel);

        // Update the timer to the new ccmpare value for this servo
        CCPR1 = TMR1 + usToTicks(servoValues[channel]);
        channel++;
    }
    else
    {
        // We've finished all the channels. Set the next compare interrupt
        // to the refresh interval. Since our timer is so slow, we should
        // never have problem doing this part, but I added the if-statement
        // as an extra precaution. This is similar to Arduino Servo source.
        if((TMR1 + 8) < usToTicks(REFRESH_US))
        {
            // Set the next compare to the refresh interval.
            // On the next interrupt we will reset the timer and start over.
            CCPR1 = usToTicks(REFRESH_US);
        }
        else
        {
            // else, at least one refresh interval has elapsed
            // The plus 8 is to ensure we don't miss the next compare while
            // we're finishing this interrupt.
            CCPR1 = TMR1 + 8;
            channel = 0;
            servoFlags |= (1 << SERVO_RESET);
        }
    }
}

void blast()
{
    blastOn(10);
    blastOff(140);
    blastOn(20);
    blastOff(130);
    blastOn(20);
    blastOff(130);
    blastOn(150);
    blastLED=0;
}

void snipe()
{
    snipeOn(10);
    snipeOff(140);
    snipeOn(20);
    snipeOff(130);
    snipeOn(20);
    snipeOff(130);
    snipeOn(150);
    snipeLED=0;
}

void blastOn(unsigned char j)
{
    for (IrTxControl=0;IrTxControl<=j;IrTxControl++)
    {
        blastLED = 1;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        blastLED = 0;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
    }
}

void blastOff(unsigned char j)
{
    for (IrTxControl=0;IrTxControl<=j;IrTxControl++)
    {
        blastLED = 1;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        blastLED = 1;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
    }
}

void snipeOn(unsigned char j)
{
    for (IrTxControl=0;IrTxControl<=j;IrTxControl++)
    {
        snipeLED = 1;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        snipeLED = 0;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
    }
}

void snipeOff(unsigned char j)
{
    for (IrTxControl=0;IrTxControl<=j;IrTxControl++)
    {
        snipeLED = 1;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        snipeLED = 1;
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
        Delay1TCY();
    }
}

void flashLED(char color, unsigned int time, int brightness){

    for (i = 0; i < 6; i++){
        if (color == 'r')
            colorLED(i, brightness, 0, 0);
        if (color == 'g')
            colorLED(i, 0, brightness, 0);
        if (color == 'b')
            colorLED(i, 0, 0, brightness);
        if (color == 'c')
            colorLED(i, 0, brightness, brightness);
        if (color == 'y')
            colorLED(i, brightness, brightness, 0);
        if (color == 'm')
            colorLED(i, brightness, 0, brightness);
        if (color == 'w')
            colorLED(i, brightness, brightness, brightness);
    } //end for

    writeLED();

    // Delay function times 5
    __delay_ms(50);
    __delay_ms(50);
    __delay_ms(50);
    __delay_ms(50);
    __delay_ms(50);
    __delay_ms(50);

    // Turn off all LEDs
    clearLED();

    __delay_ms(50);
} //end flashLED

void Cooldown( char color, int step, int loop){
    int firstLoop = 1;

    while (loop >= 0 ){
        for (i = 0; i < 5; i++){
            if (i == 0 && firstLoop)
                for (j = 0; j < MAX_STEP; j++){
                    if (color == 'r')
                        colorLED(i, j, 0, 0);
                    if (color == 'g')
                        colorLED(i, 0, j, 0);
                    if (color == 'b')
                        colorLED(i, 0, 0, j);
                    if (color == 'y')
                        colorLED(i, j, j, 0);
                    if (color == 'c')
                        colorLED(i, 0, j, j);
                    if (color == 'm')
                        colorLED(i, j, 0, j);
                    if (color == 'w')
                        colorLED(i, j, j, j);
                    writeLED();
                    j = j + step;
                    firstLoop = 0;
                } //end for
            if (i > 0){
                for (j = 0, k = MAX_STEP; j < MAX_STEP || k > 0; j++, k--){
                    if (color == 'r'){
                        colorLED(i, j, 0, 0);
                        colorLED(i-1, k, 0, 0);
                    } //end if
                    if (color == 'g'){
                        colorLED(i, 0, j, 0);
                        colorLED(i-1, 0, k, 0);
                    } //end if
                    if (color == 'b'){
                        colorLED(i, 0, 0, j);
                        colorLED(i-1, 0, 0, k);
                    } //end if
                    if (color == 'y'){
                        colorLED(i, j, j, 0);
                        colorLED(i-1, k, k, 0);
                    } //end if
                    if (color == 'c'){
                        colorLED(i, 0, j, j);
                        colorLED(i-1, 0, k, k);
                    } //end if
                    if (color == 'm'){
                        colorLED(i, j, 0, j);
                        colorLED(i-1, k, 0, k);
                    } //end if
                    if (color == 'w'){
                        colorLED(i, j, j, j);
                        colorLED(i-1, k, k, k);
                    } //end if
                    writeLED();
                    j = j + step;
                    k = k - step;
                } //end for
                colorLED(i-1, 0, 0, 0);
                writeLED();
            } //end if
            if (i == 7){
                for (j = 0, k = MAX_STEP; j < MAX_STEP, k > 0; k++, j--){
                    if (color == 'r'){
                        if (loop > 0)
                            colorLED(0, j, 0, 0);
                        colorLED(i, k, 0, 0);
                    } //end if
                    if (color == 'g'){
                        if (loop > 0)
                            colorLED(0, 0, j, 0);
                        colorLED(i, 0, k, 0);
                    } //end if
                    if (color == 'b'){
                        if (loop > 0)
                            colorLED(0, 0, 0, j);
                        colorLED(i, 0, 0, k);
                    } //end if
                    if (color == 'y'){
                        if (loop > 0)
                            colorLED(0, j, j, 0);
                        colorLED(i, k, k, 0);
                    } //end if
                    if (color == 'c'){
                        if (loop > 0)
                            colorLED(0, 0, j, j);
                        colorLED(i, 0, k, k);
                    } //end if
                    if (color == 'm'){
                        if (loop > 0)
                            colorLED(0, j, 0, j);
                        colorLED(i, k, 0, k);
                    } //end if
                    if (color == 'w'){
                        if (loop > 0)
                            colorLED(0, j, j, j);
                        colorLED(i, k, k, k);
                    } //end if
                    writeLED();
                    k = k - step;
                } //end for
                colorLED(i, 0, 0, 0);
                writeLED();
            } //end if
        } //end for
        loop--;
    } //end while
} //end cooldown

void fadeLED(char color, int step){

    for (i = 0; i < MAX_STEP; i++){
        for (j = 0; j < 8; j++){
            if (color == 'r')
                colorLED(j, i, 0, 0);
            if (color == 'g')
                colorLED(j, 0, i, 0);
            if (color == 'b')
                colorLED(j, 0, 0, i);
            if (color == 'y')
                colorLED(j, i, i, 0);
            if (color == 'c')
                colorLED(j, 0, i, i);
            if (color == 'm')
                colorLED(j, i, 0, i);
            if (color == 'w')
                colorLED(j, i, i, i);
        } //end for
        writeLED();
        i = i + step;
    } //end for
    for (i = 500; i > 0; i--){
        for (j = 0; j < 8; j++){
            if (color == 'r')
                colorLED(j, i, 0, 0);
            if (color == 'g')
                colorLED(j, 0, i, 0);
            if (color == 'b')
                colorLED(j, 0, 0, i);
            if (color == 'y')
                colorLED(j, i, i, 0);
            if (color == 'c')
                colorLED(j, 0, i, i);
            if (color == 'm')
                colorLED(j, i, 0, i);
            if (color == 'w')
                colorLED(j, i, i, i);
        } //end for
        writeLED();
        i = i - step;
    } //end for
} //end fadeLED

void colorLED(int OUTn, int red, int green, int blue){
    selectPWM(OUTn*3, red);
    selectPWM(OUTn*3+1, green);
    selectPWM(OUTn*3+2, blue);
} //end colorLED

void selectPWM(int chan, int pwm){
    if (pwm > MAX_STEP) pwm = 255;
    if (chan > 9) return;

    pwmbuffer[chan] = pwm;
} //end selectPWM

void clearLED(){

    for (i = 0; i < 9; i++)
        pwmbuffer[i] = 0;
    writeLED();

} //end clearLED

void writeLED(){
    XLAT = 0;
    for (c = 9; c >= 0; c--){
        for (b = 11; b >= 0; b--){
            SCLK = 0;
            __delay_us(5);
            if ( pwmbuffer[c] && (1 << b) )
                SIN = 1;
            else
                SIN = 0;
            SCLK = 1;
        } //end for
    } //end for
    SCLK = 0;

    BLANK = 1;
    XLAT = 1;
    BLANK = 0;
    XLAT = 0;

} //end writeLED

// ***** CANbus Functions ***********************************************************
void send_Dead(void)
{
    int dead = 1;
    TXB0EIDH = 0x00;
    TXB0EIDL = 0x00;
    TXB0SIDH = 0x00;
    TXB0SIDL = 0x00;
    TXB0DLC = 0x02;
    TXB0D0 = 0x05;
    TXB0D1 = 0xFE;
    TXB0CONbits.TXREQ = 1; //Set the buffer to transmit
    while(dead == 1){
        // Dead
    }
}
void MCAN_Receive(void)
{
    if(RXB0CONbits.RXFUL)
    {
        temp_EIDH = RXB0EIDH;
        temp_EIDL = RXB0EIDL;
        temp_SIDH = RXB0SIDH;
        temp_SIDL = RXB0SIDL;
        temp_DLC = RXB0DLC;
        temp_D0 = RXB0D0;
        temp_D1 = RXB0D1;
        RXB0CONbits.RXFUL = 0;
        __delay_ms(50);
        __delay_ms(50);
        __delay_ms(50);
        __delay_ms(50);
    }
}

void CanInit(void)
{
    TRISBbits.TRISB2 = 0;       //Set CANTX as output
    TRISBbits.TRISB3 = 1;       //Set CANRX as input
    TRISB |= 0x08;              //Configure pins to be ECAN pins
    CANCON = 0x80;              //Initial CAN to config mode

    while(!((CANSTAT&0x80) == 0x80));   //wait for CAN to be in configure mode
    ECANCONbits.MDSEL0 = 0;     //Selecting mode 0 for pic18f2580
    ECANCONbits.MDSEL1 = 0;     //Selecting mode 0 for pic18f2580
    BRGCON1 = 0x03; //Configure the baud rate of the CANBUS
    BRGCON2 = 0x83; //62.5 kb/s
    BRGCON3 = 0x01;
    //sets recieve masks for SIDs and EIDs
    RXM0EIDH = 0x00;
    RXM0EIDL = 0x00;
    RXM0SIDH = 0x00;
    RXM0SIDL = 0x00;

    RXM1EIDH = 0x00;
    RXM1EIDL = 0x00;
    RXM1SIDH = 0x00;
    RXM1SIDL = 0x00;

    RXFCON0 = 0x00;
    RXFCON1 = 0x00;
    //TXB0DLC = 0x01;// How many bytes in data field 0-8 bytes - Set to 1

    //Set CAN module to Normal Mode
    CANCON = 0x00;
    while(CANSTAT!=0); //wait for Normal Mode

    //Set Receive Mode for buffers
    RXB0CON = 0x60;
    RXB1CON = 0x60;
}